//////////////////////////////////
//
// 这是一个用于投影变换的工具类，此类只是工程实现，不涉及算法实现
//
/////////////////////////////////
#include "vision_ruler/ProjectUtil.h"

vision_ruler::ProjectUtil::ProjectUtil() {}
vision_ruler::ProjectUtil::~ProjectUtil() {}

void vision_ruler::ProjectUtil::computePose(std::vector<vision_ruler::box_info>& boxes, vision_ruler::camera_info cinfo) {
    if(boxes.size() > 0) {
        cv::Mat rvec, t, cameraMatrix, D, inliers;
        double k1 = cinfo.k1;
        double k2 = cinfo.k2;
        double k3 = cinfo.k3;
        double p1 = cinfo.p1;
        double p2 = cinfo.p2;
        double cx = cinfo.cx;
        double cy = cinfo.cy;
        double fx = cinfo.fx;
        double fy = cinfo.fy;
        cameraMatrix.create(3, 3, CV_64FC1);
        cameraMatrix.setTo(cv::Scalar(0));
        cameraMatrix.at<double>(0, 0) = fx;
        cameraMatrix.at<double>(1, 1) = fy;
        cameraMatrix.at<double>(0, 2) = cx;
        cameraMatrix.at<double>(1, 2) = cy;
        cameraMatrix.at<double>(2, 2) = 1;

        cv::Point2f left_top_pixel;
        cv::Point2f right_bottom_pixel;

        for(size_t i = 0; i < boxes.size(); i++) {
            vision_ruler::box_info & box = boxes[i];
            if(box.comefrom!=0) continue;
            if(boxes[i].isValid) {
                std::vector<cv::Point2f> pixels;
                std::vector<cv::Point3f> coordinates;
                pixels.push_back(boxes[i].left_top_pixel);
                pixels.push_back(cv::Point2f(boxes[i].left_top_pixel.x, boxes[i].right_bottom_pixel.y));
                pixels.push_back(cv::Point2f(boxes[i].right_bottom_pixel.x, boxes[i].left_top_pixel.y));
                pixels.push_back(boxes[i].right_bottom_pixel);
                coordinates.push_back(boxes[i].left_top_coordinate);
                coordinates.push_back(boxes[i].left_bottom_coordinate);
                coordinates.push_back(boxes[i].right_top_coordinate);
                coordinates.push_back(boxes[i].right_bottom_coordinate);
                cv::solvePnP(coordinates, pixels, cameraMatrix, D, rvec, t, false, cv::SOLVEPNP_EPNP);
                box.x = t.at<double>(0);
                box.y = t.at<double>(1);
                box.distance = t.at<double>(2);
                box.yaw = atan(box.x / box.distance);
            }
        }
    }
}
